#include <iostream>
#include <vector>
#include <cmath>
#include "B_spline.h"

using namespace std;

B_spline::B_spline(vector< vector<double> > Coord, vector<double> Vec, int deg)
{
	int i ;
	int n ;
	
	control_point.resize(Coord.size()) ;
	node_vector.resize(Vec.size()) ;
	for (i=0; i<=Coord.size(); i++)
	{	
		control_point[i].resize(Coord[i].size()) ;
		control_point[i]=Coord[i] ;
		//control_point()=Coord ;
	}
	node_vector=Vec ;
	degre=deg ;
	return ;
}

// Destructeur
B_spline::~B_spline()
{
}

int B_spline::findspan(double u)
{
		/* INPUT
      n - number of control points - 1
      p - spline degree
      u - parametric point
      U - knot sequence
      RETURN:
      s - knot span, localisation of the parametric point in the knot sequence
	   Algorithm A2.1 from 'The NURBS BOOK' pg68. */

      int s, ou, i, nb_vec;
		bool ok=true ;
		nb_vec=node_vector.size() ;

		i=0 ;
		if (u != node_vector[nb_vec-1])
		{
			while (ok)			
			{
				if ((node_vector[i+1]>u) and (node_vector[i]<=u))
				{
					ok=false ;
					s=i ;
				}
				if (i>=nb_vec)
				{
					ok=false ;
					s=0 ;
				}
				i++ ;
			}
		}
		else
		{
			i=nb_vec ;
			while (ok)
			{
				if ((node_vector[i-1]<u)and(node_vector[i]>=u))
				{
					ok=false ;
					s=i-1 ;
				}
				if (i<=0)
				{
					ok=false ;
					s=0 ;
				}
				i-- ;
			}
		}
      return s ;
}

vector<double> B_spline::basisfun(double u)
{
		/* INPUT:
      u - parametric Point
		OUTPUT:
      N - Basis functions vector[degre+1]

		Algorithm A2.2 from 'The NURBS BOOK' degreg70. */

		int i,j,r;
		double saved, temp;
		std::vector<double> left(degre+1) ;
		std::vector<double> right(degre+1) ;
		std::vector<double> N ;

		i=findspan(u) ;
		N.resize(degre+1) ;
		N[0] = 1.0;
		for (j = 1; j <= degre; j++)
		{
			left[j]  = u - node_vector[i+1-j];
			right[j] = node_vector[i+j] - u;
			saved = 0.0;
			for (r = 0; r < j; r++)
			{
				temp = N[r] / (right[r+1] + left[j-r]);
				N[r] = saved + right[r+1] * temp;
				saved = left[j-r] * temp;
			}
			N[j] = saved;
		}
		return N ;
}

void B_spline::bspkntins(vector<double> u)
{
		/* INPUT
		d	- Degree of the B-Spline.
		c	- Control points, matrix of size (dim,nc).
		k	- Knot sequence, row vector of size nk.
		u	- Row vector of knots to be inserted, size nu
		OUTPUT:
		ic	- Control points of the new B-Spline, of size (dim,nc+nu)
		ik	- Knot vector of the new B-Spline, of size (nk+nu)
		Algorithm A2.2 from 'The NURBS BOOK' pg70. */

		int ierr = 0;
	 	int q, j, l ,nk, i, s ;
		int nc, nu, mc, n, r, ind ;
		int m, a, b ;
		double alfa;
		//std::vector<vector<double> > c ;
		vector<double> ik ;
		vector<vector<double> > ic ;
		mc = control_point.size() ;
		nc = control_point[0].size() ;

		//ic=vector<vector<double> >[mc][nc+nu] ;
		//ik=new double[nk+nu] ;
		n = nc - 1;
		r = nu - 1;

		m = n + degre + 1;
		a = findspan(u[0]);
		b = findspan(u[r]);
		b = b++;

		for (q = 0 ; q < mc; q++)
		{
			for (j=0 ; j<=a-degre ; j++)
			{
			ic[q][j] = control_point[q][j] ;
			}
			for (j=b-1 ; j<=n ; j++)
			{
			ic[q][j+r+1] = control_point[q][j] ;
			}
		}
		for (j=0 ; j<=a ; j++)
		{
			ik[j]=node_vector[j] ;
		}
		for (j=b+degre ; j<=m;j++)
		{
			ik[j+r+1]=node_vector[j] ;
		}
		i=b+degre ;
		s=b+degre+r ;
		for (j = r ; j>=0 ; j--)
		{
			while ((u[j]<= node_vector[i]) && (i>a))
			{
				for (q=0 ; q<m ; q++)
				{
					ic[q][s-degre-1] = control_point[q][i-degre-1] ;
				}
				ik[s] = node_vector[i] ;
				s = s-1 ;
				i = i-1 ;
			}
			for (q=0 ; q<mc ; q++)
			{
				ic[q][s-degre-1] = ic[q][s-degre] ;
			}
			for (l=1 ; l<=degre ; l++)
			{
				ind = s-degre+l ;
				alfa = ik[s+l]-u[j] ;
				if (fabs(alfa ==0.0))
				{
					for (q=0 ; q<mc ; q++)
					{
						ic[q][ind-1] = ic[q][ind] ;
					}
				}
				else
				{
					alfa=alfa/(ik[s+l]-node_vector[i-degre+l]) ;
					for (q=0 ; q<mc ; q++)
					{
						ic[q][ind-1] = alfa*ic[q][ind-1]+(1.0-alfa)*ic[q][ind] ;
					}
				}
			}
			ik[s] = u[j] ;
			s = s-1 ;
		}
}


void B_spline::affiche()
{
	int i,j ;
	cout << "Point de control" << endl ;
	for (i=0 ; i < control_point.size(); i++)
	{
		for (j=0; j<control_point[i].size() ; j++)
		{
			cout << control_point[i][j] << " " ;
		}
		cout << endl ;	
	}
	cout << "Node vector" << endl ;
	for (i=0 ; i < node_vector.size() ; i++)
	{
		cout << node_vector[i] << " " ;
	}
	cout << endl ;	
	cout << "Degres de la Nurbs" << endl ;
	cout << degre << endl ;
}

/* 
int bspdegelev(int d, double *c, int mc, int nc, double *k, int nk, int t, int *nh, double *ic, double *ik) 
{ 
/*
	Function Name: 
		bspdegevel - Degree elevate a univariate B-Spline. 
	Calling Sequence: 
		 [ic,ik] = bspdegelev(d,c,k,t) 
	Parameters: 
		 d	: Degree of the B-Spline. 
		 c	: Control points, matrix of size (dim,nc). 
		 k	: Knot sequence, row vector of size nk. 
		 t	: Raise the B-Spline degree t times. 
		 ic: Control points of the new B-Spline. 
		 ik: Knot vector of the new B-Spline. 
	Description: 
		Degree elevate a univariate B-Spline. This function provides an 
		interface to a toolbox 'C' routine. */
/*

   int row,col; 
 
   int ierr = 0; 
   int i, j, q, s, m, ph, ph2, mpi, mh, r, a, b, cind, oldr, mul; 
   int n, lbz, rbz, save, tr, kj, first, kind, last, bet, ii; 
   double inv, ua, ub, numer, den, alf, gam; 
   double **bezalfs, **bpts, **ebpts, **Nextbpts, *alfs; 
 
   double **ctrl  = vec2mat(c, mc, nc); 
   double **ictrl = vec2mat(ic, mc, nc*(t+1)); 
 
   n = nc - 1; 
 
   bezalfs = matrix(d+1,d+t+1); 
   bpts = matrix(mc,d+1); 
   ebpts = matrix(mc,d+t+1); 
   Nextbpts = matrix(mc,d+1); 
   alfs = (double *) mxMalloc(d*sizeof(double)); 
 
   m = n + d + 1; 
   ph = d + t; 
   ph2 = ph / 2; 
 
   // compute bezier degree elevation coefficeients 
   bezalfs[0][0] = bezalfs[ph][d] = 1.0; 
 

   for (i = 1; i <= ph2; i++) { 
     inv = 1.0 / bincoeff(ph,i); 
     mpi = min(d,i); 
 
     for (j = max(0,i-t); j <= mpi; j++) 
 %       bezalfs[i][j] = inv * bincoeff(d,j) * bincoeff(t,i-j); 
   
   } 
 
   for (i = ph2+1; i <= ph-1; i++) { 
     mpi = min(d, i); 
     for (j = max(0,i-t); j <= mpi; j++) 
       bezalfs[i][j] = bezalfs[ph-i][d-j]; 
   
   } 
 
   mh = ph;       
   kind = ph+1; 
   r = -1; 
   a = d; 
   b = d+1; 
   cind = 1; 
   ua = k[0];  
 
   for (ii = 0; ii < mc; ii++) 
     ictrl[0][ii] = ctrl[0][ii]; 
 
   for (i = 0; i <= ph; i++) 
     ik[i] = ua; 
 
   // initialise first bezier seg 
   for (i = 0; i <= d; i++) 
     for (ii = 0; ii < mc; ii++) 
       bpts[i][ii] = ctrl[i][ii]; 
   
 
   // big loop thru knot vector 
   while (b < m)  { 
     i = b; 
     while (b < m && k[b] == k[b+1]) 
       b++; 
 
     mul = b - i + 1; 
     mh += mul + t; 
     ub = k[b]; 
     oldr = r; 
     r = d - mul; 
 
     // insert knot u(b) r times 
     if (oldr > 0) 
       lbz = (oldr+2) / 2; 
     else 
       lbz = 1; 
 

     if (r > 0) 
       rbz = ph - (r+1)/2; 
     else 
       rbz = ph; 
 

     if (r > 0) { 
       // insert knot to get bezier segment 
       numer = ub - ua; 
       for (q = d; q > mul; q--) 
         alfs[q-mul-1] = numer / (k[a+q]-ua); 


       for (j = 1; j <= r; j++)  { 
         save = r - j; 
         s = mul + j; 
 
         for (q = d; q >= s; q--) 
           for (ii = 0; ii < mc; ii++) 


             bpts[q][ii] = alfs[q-s]*bpts[q][ii]+(1.0-alfs[q-s])*bpts[q-1][ii]; 
   
 

         for (ii = 0; ii < mc; ii++) 
           Nextbpts[save][ii] = bpts[d][ii]; 
   
       } 
     } 
     // end of insert knot 
 
     // degree elevate bezier 
     for (i = lbz; i <= ph; i++)  { 
       for (ii = 0; ii < mc; ii++) 
         ebpts[i][ii] = 0.0; 
   
       mpi = min(d, i); 
       for (j = max(0,i-t); j <= mpi; j++) 
         for (ii = 0; ii < mc; ii++) 


           ebpts[i][ii] = ebpts[i][ii] + bezalfs[i][j]*bpts[j][ii]; 
   
   
     } 
     // end of degree elevating bezier 
 
     if (oldr > 1)  { 
       // must remove knot u=k[a] oldr times 
                %       first = kind - 2; 
       last = kind; 
       den = ub - ua; 
       bet = (ub-ik[kind-1]) / den; 
 
       // knot removal loop 
       for (tr = 1; tr < oldr; tr++)  { 
         i = first; 
         j = last; 
         kj = j - kind + 1; 
         while (j - i > tr)  { 
           // loop and compute the new control points 
           // for one removal step 
           if (i < cind)  { 
             alf = (ub-ik[i])/(ua-ik[i]); 
             for (ii = 0; ii < mc; ii++) 


               ictrl[i][ii] = alf * ictrl[i][ii] + (1.0-alf) * ictrl[i-1][ii]; 
   
           } 
           if (j >= lbz)  { 
             if (j-tr <= kind-ph+oldr) { 
               gam = (ub-ik[j-tr]) / den; 
               for (ii = 0; ii < mc; ii++) 


                 ebpts[kj][ii] = gam*ebpts[kj][ii] + (1.0-gam)*ebpts[kj+1][ii]; 
             } 
             else  { 
               for (ii = 0; ii < mc; ii++) 
                            
                                
                 ebpts[kj][ii] = bet*ebpts[kj][ii] + (1.0-bet)*ebpts[kj+1][ii]; 
   
             } 
           } 
           i++; 
           j--; 
           kj--; 
         } 
 
         first--; 
         last++; 
       } 
     } 
     // end of removing knot n=k[a] 
 
     // load the knot ua 
     if (a != d) 
       for (i = 0; i < ph-oldr; i++)  { 
         ik[kind] = ua; 
         kind++; 

       } 
 
     // load ctrl pts into ic 
     for (j = lbz; j <= rbz; j++)  { 
       for (ii = 0; ii < mc; ii++) 
         ictrl[cind][ii] = ebpts[j][ii]; 
   
       cind++; 
     } 
 
     if (b < m)  { 
       // setup for next pass thru loop 
       for (j = 0; j < r; j++) 
         for (ii = 0; ii < mc; ii++) 
           bpts[j][ii] = Nextbpts[j][ii]; 


       for (j = r; j <= d; j++) 
         for (ii = 0; ii < mc; ii++) 
           bpts[j][ii] = ctrl[b-d+j][ii]; 
   
   
       a = b; 
       b++; 
       ua = ub; 
     } 
     else 
       // end knot 
       for (i = 0; i <= ph; i++) 
         ik[kind+i] = ub; 
   
   
   } 
   // end while loop 
 
   *nh = mh - ph - 1; 
 
   freevec2mat(ctrl); 
   freevec2mat(ictrl); 
   freematrix(bezalfs); 
   freematrix(bpts); 
   freematrix(ebpts); 
   freematrix(Nextbpts); 
   mxFree(alfs); 
 
   return(ierr); 
 } 
 */
/*
double bincoeff(int n, int k) 
{ /*  Computes the binomial coefficient. 
 
      ( n )      n! 
      (   ) = -------- 
      ( k )   k!(n-k)! 
 
		b = bincoeff(n,k) 
 
		Algorithm from 'Numerical Recipes in C, 2nd Edition' pg215. */
/*
	return floor(0.5+exp(factln(n)-factln(k)-factln(n-k))); 
} */
/* 
double factln(double n) 
{
	// computes ln(n!) 
	if (n <= 1) 
	{
		f = 0; 
		return f
	} 
	else
	{
		f = log(factorial(n));
	}
}*/


// Attention, marche pas, Je ne sais si c'est utile!!
void B_spline::bspderiv(int d) 
{
/* 
	Function Name:  
		 bspdeval - Evaluate the control points and knot sequence of the derivative 
             of a univariate B-Spline. 
	Calling Sequence: 
		[dc,dk] = bspderiv(d,c,k) 
	Parameters: 
	    d	: Degree of the B-Spline.  
		 c	: Control Points, matrix of size (dim,nc). 
	    k	: Knot sequence, row vector of size nk. 
	   dc	: Control points of the derivative 
	   dk	: Knot sequence of the derivative 
	Description: 
	   Evaluate the derivative of univariate B-Spline, which is itself a B-Spline. 
		This function provides an interface to a toolbox 'C' routine. */
	int mc,nc,nk ;
	int ierr = 0; 
	int i, j, tmp; 
	vector< vector<double> > dc ;
	vector<double> dk ;


	mc=control_point.size() ;
	nc=control_point[0].size() ;
	nk=node_vector.size() ;

	// Allocation memoire
	dc.resize(mc) ;
	for (i=0 ; i<nc ; i++)
	{
		dc[i].resize(nc) ;
	}
	
	// Construction des derives
	for (i = 0; i < nc; i++) 
	{ 
		tmp = degre / (node_vector[i+degre+1] - node_vector[i+1]); 
		for (j = 0; j < mc; j++) 
		{ 
			dc[i][j] = tmp * (control_point[i+1][j] - control_point[i][j]); 
		} 
	} 

	dk.resize(nk-1) ;
	// construction des fonctions
	j = 0; 
	for (i = 1; i < nk-1; i++)
	{ 
		 dk[j++] = node_vector[i]; 
	}
	return  ; 
}

// Derivees des fonctions de formes
vector< vector<double> > B_spline::derbasisfuns(int nders,double u)
{
	/*
	Function Name: 
		bspdegevel - Degree elevate a univariate B-Spline. 
	Calling Sequence: 
		 [Ndu] = derbasisfuns(d,c,k,t) 
	Parameters: 
		i = numéro de la fonction à calculer (sortie de findspan)
		pl = degrés de la nurbs
		u = endroit ou l'on veut la fonction
		nders = numéro de la dérivée désirée
		U = vecteur de noeud de la fonction
	Description: 
		Degree elevate a univariate B-Spline. This function provides an 
		interface to a toolbox 'C' routine. */
	int i,j,k,r,n ;
	int j2,j1,s2,s1,rk,pk ;
	vector< vector<double> > Ndu,a,ders ;
	vector<double> left,right ;
	double temp,d,saved ;
	
	i=findspan(u) ;

	// Allocation memoire
	Ndu.resize(nders) ;
	a.resize(nders) ;
	ders.resize(nders) ;
	left.resize(degre) ;
	right.resize(degre) ;

	for (j=0 ; j<nders+1 ; j++ )
	{
		Ndu[j].resize(degre) ;
		a[j].resize(degre) ;
		ders[j].resize(degre) ;
		
		for (r=0 ; r<degre ; r++)
		{
			Ndu[j][r]=0.0 ;
			a[j][r]=0.0 ;
			ders[j][r]=0.0 ;
		}
	}

	Ndu[0][0]=1.0 ;
	for (j=1; j<=degre ; j++)
	{
		left[j]=u-node_vector[i+j] ;	
		right[j]=node_vector[i+j+1] ;
		saved=0.0 ;
		for (r=0;r<j;r++)
		{
			Ndu[j][r]=right[r+1]+left[j-r] ;
			temp=Ndu[r][j-1]/Ndu[j][r] ;
		
			Ndu[r][j] = saved+right[r+1]*temp ;
			saved = left[j-r]*temp ;
		}
		Ndu[j][j] = saved ; 
	}	

	for (j=0;j<=degre;j++)
	{
		ders[0][j]=Ndu[j][degre] ;
	}
	for (r=0;r<j;r++)
	{
		s1=0;
		s2=1;
		a[0][0]=1.0 ;
		for (k=1; k<=n ; k++)
		{
			d=0.0;
			rk=r-k;
			pk=degre-k;
			if (r>=k)
			{
				a[s2][0]=a[s1][0]/Ndu[pk+1][rk] ;
				d=a[s2][0]*Ndu[rk][pk] ;
			}
			if (rk >= -1)
			{
				j1 = 1;
			}
			else
			{
				j1=-rk ;
			}
			if (rk <= pk)
			{
				j2 = k-1;
			}
			else
			{
				j2=degre-r ;
			}
			for (j=j1;j<=j2;j++)
			{
				a[s2][j]=(a[s1][j]-a[s1][j-1])/Ndu[pk+1][rk+j] ;
				d+=a[s2][j]*Ndu[rk+1][rk] ;
			}
			if (rk <= pk)
			{
				a[s2][k]=-a[s1][k-1]/Ndu[pk+1][r];
				d+=a[s2][k]*Ndu[r][pk];
			}
			ders[k][r]=d ;
			j=s1 ;
			s1=s2 ;
			s2=j ;
		}
	}
	r=degre ;
	for (k=1; k<=n;k++)
	{
		for (j=0 ;j<=degre ;j++)
		{
			ders[k][j]*=r ;
			r*=(degre-k) ;
		}
	}
}
